/*
 *  Mathutilis.cpp
 *  c_Render
 *
 *  Created by liao peterson on 11/8/09.
 *  Copyright 2009 uestc. All rights reserved.
 *
 */

#include "Mathutilis.h"

Quat Quat::operator*(const Quat&times) const
{
	Quat result;
	result.w=w*times.w-x*times.x-y*times.y-z*times.z;
	result.x=w*times.x+x*times.w+y*times.z-z*times.y;
	result.y=w*times.y+y*times.w+z*times.x-x*times.z;
	result.z=w*times.z+z*times.w+x*times.y-y*times.x;
	
	return result;
}

Quat Quat::operator/(const Quat&times) const
{
	Quat result;
	GLfloat norm=times.w*times.w+times.x*times.x
				+times.y*times.y+times.z*times.z;
	result.w=times.w/norm;
	result.x=-times.x/norm;
	result.y=-times.y/norm;
	result.z=-times.z/norm;
	
	return (*this)*result;
}

Quat Quat::operator+(const Quat&addOn) const
{
	 Quat result;
	 result.w=w+addOn.w;
	 result.x=x+addOn.x;
	 result.y=y+addOn.y;
	 result.z=z+addOn.z;
 
	 return result;
}

Quat Quat::operator-(const Quat&addOn) const
{
	Quat result;
	result.w=w-addOn.w;
	result.x=x-addOn.x;
	result.y=y-addOn.y;
	result.z=z-addOn.z;
	
	return result;
}


Quat::Quat()
{
	x=0;y=0;z=0;w=1;
}

Quat::Quat(GLfloat*q)
{
	if(!q)return;
	w=-q[0];x=q[1];y=q[2];z=q[3];
}

Quat::Quat(GLfloat Nw,GLfloat Nx,GLfloat Ny,GLfloat Nz)
{
	x=Nx;
	y=Ny;
	z=Nz;
	w=Nw;
}

Quat::Quat(vec3 &axis, GLfloat theta)
{
	GLfloat n = axis.norm();
	if(n<EPSILON)
	{
		w=1;
		x=0;
		y=0;
		z=0;
		return;
	}
	w = cosf(theta/2);
	GLfloat t = sinf(theta/2)/n;
	x = axis.x*t;
	y = axis.y*t;
	z = axis.z*t;
}

void Quat::Set(GLfloat*q)
{
	if(!q)return;
	w=-q[0];x=q[1];y=q[2];z=q[3];
}

void Quat::Set(GLfloat Nw,GLfloat Nx,GLfloat Ny,GLfloat Nz)
{
	x=Nx;
	y=Ny;
	z=Nz;
	w=Nw;
}

void Quat::Set(const vec3 &axis, GLfloat theta)
{
	GLfloat n = axis.norm();
	if(n<EPSILON/1000)
		return;
	
	w = cosf(theta/2);
	GLfloat t = sinf(theta/2)/n;
	x = axis.x*t;
	y = axis.y*t;
	z = axis.z*t;
}

//void Quat::Conjugate()
//{

	
//////////////////////////////////////////////////////////////////////////////////////////////////
/*
 /*			Implementation for class Matrix3x3
 */
//////////////////////////////////////////////////////////////////////////////////////////////////
Matrix3x3::Matrix3x3()
{
	m=(GLfloat*)malloc(sizeof(GLfloat)*9);
}

Matrix3x3::Matrix3x3(const Matrix3x3*copy)
{
	if(!copy)return;
	m=(GLfloat*)malloc(sizeof(GLfloat)*9);
	memcpy(m, copy->m, sizeof(GLfloat)*9);
}

Matrix3x3::~Matrix3x3()
{
	free(m);
}

void Matrix3x3::Copy3x3(const Matrix3x3*target)
{
	memcpy(m, target->m, 9*sizeof(GLfloat));
}

void Matrix3x3::SetIdentity()
{
	memset(m,0,9*sizeof(GLfloat));
	m[0] = 1.0f;
	m[5] = 1.0f;
	m[10] = 1.0f;
}

void Matrix3x3::SetRotY(const GLfloat angle)
{
	const GLfloat c = cosf(angle);
	const GLfloat s = sinf(angle);
	
	m[0] = c;
	m[1] = 0;
	m[2] = -s;
	
	m[3] = 0;
	m[4] = 1;
	m[5] = 0;
	
	m[6] = s;
	m[7] = 0;
	m[8] = c;
}

void Matrix3x3::SetRotX(const GLfloat angle)
{
	const GLfloat c = cosf(angle);
	const GLfloat s = sinf(angle);
	
	m[0] = 1;
	m[1] = 0;
	m[2] = 0;
	
	m[3] = 0;
	m[4] = c;
	m[5] = s;
	
	m[6] = 0;
	m[7] = -s;
	m[8] = c;
}

void Matrix3x3::SetRotZ(const GLfloat angle)
{
	const GLfloat c = cosf(angle);
	const GLfloat s = sinf(angle);
	
	m[0] = c;
	m[1] = -s;
	m[2] = 0;
	
	m[3] = s;
	m[4] = c;
	m[5] = 0;
	
	m[6] = 0;
	m[7] = 0;
	m[8] = 1;
}


void Matrix3x3::SetRotXYZ(const GLfloat angleX,const GLfloat angleY,const GLfloat angleZ)
{
	const GLfloat c1 = cosf(angleX);
	const GLfloat s1 = sinf(angleX);
	
	const GLfloat c2 = cosf(angleY);
	const GLfloat s2 = sinf(angleY);	
	
	const GLfloat c3 = cosf(angleZ);
	const GLfloat s3 = sinf(angleZ);	
	
	m[0] = c2*c3;
	m[1] = s1*s2*c3-c1*s3;
	m[2] = c1*c3*s2+s1*s3;
	
	m[3] = c2*s3;
	m[4] = s1*s2*s3+c1*c3;
	m[5] = c1*s2*s3-s1*c3;
	
	m[6] = -s2;
	m[7] = s1*c2;
	m[8] = c1*c2;
}

void Matrix3x3::SetRotZYX(const GLfloat angleX,const GLfloat angleY,const GLfloat angleZ)
{
	const GLfloat c1 = cosf(angleX);
	const GLfloat s1 = sinf(angleX);
	
	const GLfloat c2 = cosf(angleY);
	const GLfloat s2 = sinf(angleY);	
	
	const GLfloat c3 = cosf(angleZ);
	const GLfloat s3 = sinf(angleZ);	
	
	m[0] = c2*c3;
	m[1] = -s3*s2;
	m[2] = s2;
	
	m[3] = c3*s1*s2+s3*c1;
	m[4] = -s3*s2*s1+c3*c1;
	m[5] = -c2*s1;
	
	m[6] = -s2*c1*c3+s3*s1;
	m[7] = s2*c1*s3+c3*s1;
	m[8] = c1*c2;
}



void Matrix3x3::SetRotQuat(const Quat*q)
{
	if(!q)return;
	const GLfloat a=q->w;
	const GLfloat b=q->x;
	const GLfloat c=q->y;
	const GLfloat d=q->z;
	
	m[0]=a*a+b*b-c*c-d*d;
	m[1]=2*b*c-2*a*d;
	m[2]=2*b*d+2*a*c;
	
	m[3]=2*b*c+2*a*d;
	m[4]=a*a-b*b+c*c-d*d;
	m[5]=2*c*d-2*a*b;
	
	m[6]=2*b*d-2*a*c;
	m[7]=2*c*d+2*a*b;
	m[8]=a*a-b*b-c*c+d*d;
}

// Inverse quaternion transformation
void Matrix3x3::SetRotQuatI(const Quat*q)
{
	if(!q)return;
	const GLfloat a=q->w;
	const GLfloat b=-q->x;
	const GLfloat c=-q->y;
	const GLfloat d=-q->z;
	
	m[0]=a*a+b*b-c*c-d*d;
	m[1]=2*b*c-2*a*d;
	m[2]=2*b*d+2*a*c;
	
	m[3]=2*b*c+2*a*d;
	m[4]=a*a-b*b+c*c-d*d;
	m[5]=2*c*d-2*a*b;
	
	m[6]=2*b*d-2*a*c;
	m[7]=2*c*d+2*a*b;
	m[8]=a*a-b*b-c*c+d*d;
}


Matrix3x3 Matrix3x3::operator+(const Matrix3x3&b) const
{
	Matrix3x3 result;
	
	for(int i=0;i<9;i++)
		result.m[i]=m[i]+b.m[i];
	
	return result;
}

Matrix3x3 Matrix3x3::operator-(const Matrix3x3&b) const
{
	Matrix3x3 result;
	
	for(int i=0;i<9;i++)
		result.m[i]=m[i]+b.m[i];
	
	return result;
}

//A*B
Matrix3x3 Matrix3x3::operator*(const Matrix3x3&b) const
{
	Matrix3x3 result;
	
	result.m[0] = m[0] * b.m[0] + m[3] * b.m[1] + m[6] * b.m[2];
	result.m[1] = m[1] * b.m[0] + m[4] * b.m[1] + m[7] * b.m[2];
	result.m[2] = m[2] * b.m[0] + m[5] * b.m[1] + m[8] * b.m[2];
	
	result.m[3] = m[0] * b.m[3] + m[3] * b.m[4] + m[6] * b.m[5];
	result.m[4] = m[1] * b.m[3] + m[4] * b.m[4] + m[7] * b.m[5];
	result.m[5] = m[2] * b.m[3] + m[5] * b.m[4] + m[8] * b.m[5];
	
	result.m[6] = m[0] * b.m[6] + m[3] * b.m[7] + m[6] * b.m[8];
	result.m[7] = m[1] * b.m[6] + m[4] * b.m[7] + m[7] * b.m[8];
	result.m[8] = m[2] * b.m[6] + m[5] * b.m[7] + m[8] * b.m[8];

	return result;
}

// [x x x]   [x]
// [x x x] * [x]
// [x x x]   [x]
vec3 Matrix3x3::operator*(const vec3 mul) const
{
	vec3 result;
	
	result.x = m[0]*mul.x + m[3]*mul.y + m[6]*mul.z;
	result.y = m[1]*mul.x + m[4]*mul.y + m[7]*mul.z;
	result.z = m[2]*mul.x + m[5]*mul.y + m[8]*mul.z;
	
	return result;
}

GLfloat Matrix3x3::det() const
{
	return m[0]*(m[4]*m[8]-m[5]*m[7])
			-m[3]*(m[1]*m[8]-m[2]*m[7])
			+m[6]*(m[1]*m[5]-m[2]*m[4]);
}

Matrix3x3  Matrix3x3::Inverse() const
{
	GLfloat d = det();
	if(fabs(d)<EPSILON){
		perror("Invalid Matrix Inversion\n");
		return NULL;
	}
	
	Matrix3x3 result;
	result.m[0] = (m[4] * m[8] - m[5] * m[7])/d;
	result.m[1] = (m[7] * m[2] - m[8] * m[1])/d;
	result.m[2] = (m[1] * m[5] - m[4] * m[2])/d;
	
	result.m[3] = (m[6] * m[5] - m[3] * m[8])/d;
	result.m[4] = (m[0] * m[8] - m[2] * m[6])/d;
	result.m[5] = (m[3] * m[2] - m[0] * m[5])/d;
	
	result.m[6] = (m[3] * m[7] - m[4] * m[6])/d;
	result.m[7] = (m[6] * m[1] - m[0] * m[7])/d;
	result.m[8] = (m[0] * m[4] - m[3] * m[1])/d;
	
	return result;
}

//		 [compOne  ]
//Set to [compTwo  ]	each vector occupy a row
//		 [compThree] 
void Matrix3x3::setComponent(const vec3 &compOne, const vec3 &compTwo, const vec3 &compThree)
{
	m[0] = compOne.x;
	m[1] = compOne.y;
	m[2] = compOne.z;
	
	m[3] = compTwo.x;
	m[4] = compTwo.y;
	m[5] = compTwo.z;
	
	m[6] = compThree.x;
	m[7] = compThree.y;
	m[8] = compThree.z;
}

Matrix3x3 Matrix3x3::transpose() const
{
	Matrix3x3 result;
	
	result.m[0] = m[0];
	result.m[1] = m[3];
	result.m[2] = m[6];
	
	result.m[3] = m[1];
	result.m[4] = m[4];
	result.m[5] = m[7];
	
	result.m[6] = m[2];
	result.m[7] = m[5];
	result.m[8] = m[8];
	
	return result;
}
//////////////////////////////////////////////////////////////////////////////////////////////////
/*
/*			Implementation for class Matrix4x4
*/
//////////////////////////////////////////////////////////////////////////////////////////////////
Matrix4x4::Matrix4x4()
{
	m=(GLfloat*)malloc(sizeof(GLfloat)*16);
	memset(m,0,16*sizeof(GLfloat));
}

Matrix4x4::Matrix4x4(const Matrix4x4*copy)
{
	if(!copy)return;
	m=(GLfloat*)malloc(sizeof(GLfloat)*16);
	memcpy(m, copy->m, sizeof(GLfloat)*16);
}

void Matrix4x4::Copy4x4(const Matrix4x4*target)
{
	memcpy(m, target->m, sizeof(GLfloat)*16);
}

Matrix4x4::~Matrix4x4()
{
	free(m);
}

void Matrix4x4::SetIdentity()
{
	memset(m,0,16*sizeof(GLfloat));
	m[0] = 1.0f;
	m[5] = 1.0f;
	m[10] = 1.0f;
	m[15] = 1.0f;
}

void Matrix4x4::SetOrtho(const GLfloat left,const GLfloat right,const GLfloat bottom,const GLfloat top,const GLfloat near,const GLfloat far)
{
	const GLfloat tx = - (right + left)/(right - left);
	const GLfloat ty = - (top + bottom)/(top - bottom);
	const GLfloat tz = - near/(far - near);
	
	m[0] = 2.0f/(right-left);
	m[1] = 0;
	m[2] = 0;
	m[3] = 0;
	
	m[4] = 0;
	m[5] = 2.0f/(top-bottom);
	m[6] = 0;
	m[7] = 0;
	
	m[8] = 0;
	m[9] = 0;
	m[10] = 1.0/(far-near);
	m[11] = 0;
	
	m[12] = tx;
	m[13] = ty;
	m[14] = tz;
	m[15] = 1;
}

void Matrix4x4::SetTranslate(const GLfloat shiftX,const GLfloat shiftY,const GLfloat shiftZ)
{
	m[0] = 1;
	m[1] = 0;
	m[2] = 0;
	m[3] = 0;
	
	m[4] = 0;
	m[5] = 1;
	m[6] = 0;
	m[7] = 0;
	
	m[8] = 0;
	m[9] = 0;
	m[10] = 1;
	m[11] = 0;
	
	m[12] = shiftX;
	m[13] = shiftY;
	m[14] = shiftZ;
	m[15] = 1;	
}

void Matrix4x4::SetScale(const GLfloat scaleX,const GLfloat scaleY,const GLfloat scaleZ)
{
	m[0] = scaleX;
	m[1] = 0;
	m[2] = 0;
	m[3] = 0;
	
	m[4] = 0;
	m[5] = scaleY;
	m[6] = 0;
	m[7] = 0;
	
	m[8] = 0;
	m[9] = 0;
	m[10] = scaleZ;
	m[11] = 0;
	
	m[12] = 0;
	m[13] = 0;
	m[14] = 0;
	m[15] = 1;	
}

void Matrix4x4::SetRotY(const GLfloat angle)
{
	const GLfloat c = cosf(angle);
	const GLfloat s = sinf(angle);
	
	m[0] = c;
	m[1] = 0;
	m[2] = -s;
	m[3] = 0;
	
	m[4] = 0;
	m[5] = 1;
	m[6] = 0;
	m[7] = 0;
	
	m[8] = s;
	m[9] = 0;
	m[10] = c;
	m[11] = 0;
	
	m[12] = 0;
	m[13] = 0;
	m[14] = 0;
	m[15] = 1;	
}

void Matrix4x4::SetRotX(const GLfloat angle)
{
	const GLfloat c = cosf(angle);
	const GLfloat s = sinf(angle);
	
	m[0] = 1;
	m[1] = 0;
	m[2] = 0;
	m[3] = 0;
	
	m[4] = 0;
	m[5] = c;
	m[6] = s;
	m[7] = 0;
	
	m[8] = 0;
	m[9] = -s;
	m[10] = c;
	m[11] = 0;
	
	m[12] = 0;
	m[13] = 0;
	m[14] = 0;
	m[15] = 1;
}

void Matrix4x4::SetRotZ(const GLfloat angle)
{
	const GLfloat c = cosf(angle);
	const GLfloat s = sinf(angle);
	
	m[0] = c;
	m[1] = -s;
	m[2] = 0;
	m[3] = 0;
	
	m[4] = s;
	m[5] = c;
	m[6] = 0;
	m[7] = 0;
	
	m[8] = 0;
	m[9] = 0;
	m[10] = 1;
	m[11] = 0;
	
	m[12] = 0;
	m[13] = 0;
	m[14] = 0;
	m[15] = 1;	
}


void Matrix4x4::SetRotXYZ(const GLfloat angleX,const GLfloat angleY,const GLfloat angleZ)
{
	const GLfloat c1 = cosf(angleX);
	const GLfloat s1 = sinf(angleX);
	
	const GLfloat c2 = cosf(angleY);
	const GLfloat s2 = sinf(angleY);	
	
	const GLfloat c3 = cosf(angleZ);
	const GLfloat s3 = sinf(angleZ);	
	
	m[0] = c2*c3;
	m[1] = s1*s2*c3-c1*s3;
	m[2] = c1*c3*s2+s1*s3;
	m[3] = 0;
	
	m[4] = c2*s3;
	m[5] = s1*s2*s3+c1*c3;
	m[6] = c1*s2*s3-s1*c3;
	m[7] = 0;
	
	m[8] = -s2;
	m[9] = s1*c2;
	m[10] = c1*c2;
	m[11] = 0;
	
	m[12] = 0;
	m[13] = 0;
	m[14] = 0;
	m[15] = 1;	
}

void Matrix4x4::SetRotZYX(const GLfloat angleX,const GLfloat angleY,const GLfloat angleZ)
{
	const GLfloat c1 = cosf(angleX);
	const GLfloat s1 = sinf(angleX);
	
	const GLfloat c2 = cosf(angleY);
	const GLfloat s2 = sinf(angleY);	
	
	const GLfloat c3 = cosf(angleZ);
	const GLfloat s3 = sinf(angleZ);	
	
	m[0] = c2*c3;
	m[1] = -s3*s2;
	m[2] = s2;
	m[3] = 0;
	
	m[4] = c3*s1*s2+s3*c1;
	m[5] = -s3*s2*s1+c3*c1;
	m[6] = -c2*s1;
	m[7] = 0;
	
	m[8] = -s2*c1*c3+s3*s1;
	m[9] = s2*c1*s3+c3*s1;
	m[10] = c1*c2;
	m[11] = 0;
	
	m[12] = 0;
	m[13] = 0;
	m[14] = 0;
	m[15] = 1;	
}

	

void Matrix4x4::SetRotQuat(const Quat*q)
{
	if(!q)return;
	const GLfloat a=q->w;
	const GLfloat b=q->x;
	const GLfloat c=q->y;
	const GLfloat d=q->z;
	
	m[0]=a*a+b*b-c*c-d*d;
	m[1]=2*b*c-2*a*d;
	m[2]=2*b*d+2*a*c;
	m[3]=0;
	
	m[4]=2*b*c+2*a*d;
	m[5]=a*a-b*b+c*c-d*d;
	m[6]=2*c*d-2*a*b;
	m[7]=0;
	
	m[8]=2*b*d-2*a*c;
	m[9]=2*c*d+2*a*b;
	m[10]=a*a-b*b-c*c+d*d;
	m[11]=0;
	
	m[12]=0;
	m[13]=0;
	m[14]=0;
	m[15]=1;
}

void Matrix4x4::SetRotQuatI(const Quat*q)
{
	if(!q)return;
	const GLfloat a=q->w;
	const GLfloat b=-q->x;
	const GLfloat c=-q->y;
	const GLfloat d=-q->z;
	
	m[0]=a*a+b*b-c*c-d*d;
	m[1]=2*b*c-2*a*d;
	m[2]=2*b*d+2*a*c;
	m[3]=0;
	
	m[4]=2*b*c+2*a*d;
	m[5]=a*a-b*b+c*c-d*d;
	m[6]=2*c*d-2*a*b;
	m[7]=0;
	
	m[8]=2*b*d-2*a*c;
	m[9]=2*c*d+2*a*b;
	m[10]=a*a-b*b-c*c+d*d;
	m[11]=0;
	
	m[12]=0;
	m[13]=0;
	m[14]=0;
	m[15]=1;
}




void Matrix4x4::SetPers(const GLfloat r,const GLfloat n,const GLfloat f)
{
	m[0] = 1/r;
	m[1] = 0;
	m[2] = 0;
	m[3] = 0;
	
	m[4] = 0;
	m[5] = 1;
	m[6] = 0;
	m[7] = 0;
	
	m[8] = 0;
	m[9] = 0;
	m[10] = 1/(f-n);
	m[11] = -1;
	
	m[12] = 0;
	m[13] = 0;
	m[14] = -n/(f-n);
	m[15] = 0;	
}

void Matrix4x4::operator=(const Matrix4x4&b)
{
	if(!m){
		m = (GLfloat*)malloc(16*sizeof(GLfloat));
	}
	memcpy(m, b.m, sizeof(GLfloat)*16);
}
Matrix4x4 Matrix4x4::operator+(const Matrix4x4&b) const
{
	Matrix4x4 result;
	
	for(int i=0;i<16;i++)
		result.m[i]=m[i]+b.m[i];

	return result;
}

Matrix4x4 Matrix4x4::operator-(const Matrix4x4&b) const
{
	Matrix4x4 result;
	
	for(int i=0;i<16;i++)
		result.m[i]=m[i]+b.m[i];
	
	return result;
}

//A*B
Matrix4x4 Matrix4x4::operator*(const Matrix4x4&b) const
{
	//if(b.m==NULL)
	//	return NULL;
	Matrix4x4 result;
	
	result.m[0] = b.m[0]*m[0] + b.m[1]*m[4] + b.m[2]*m[8] + b.m[3]*m[12];
	result.m[1] = b.m[0]*m[1] + b.m[1]*m[5] + b.m[2]*m[9] + b.m[3]*m[13];
	result.m[2] = b.m[0]*m[2] + b.m[1]*m[6] + b.m[2]*m[10] + b.m[3]*m[14];
	result.m[3] = b.m[0]*m[3] + b.m[1]*m[7] + b.m[2]*m[11] + b.m[3]*m[15];
	
	result.m[4] = b.m[4]*m[0] + b.m[5]*m[4] + b.m[6]*m[8] + b.m[7]*m[12];
	result.m[5] = b.m[4]*m[1] + b.m[5]*m[5] + b.m[6]*m[9] + b.m[7]*m[13];
	result.m[6] = b.m[4]*m[2] + b.m[5]*m[6] + b.m[6]*m[10] + b.m[7]*m[14];
	result.m[7] = b.m[4]*m[3] + b.m[5]*m[7] + b.m[6]*m[11] + b.m[7]*m[15];
	
	result.m[8] = b.m[8]*m[0] + b.m[9]*m[4] + b.m[10]*m[8] + b.m[11]*m[12];
	result.m[9] = b.m[8]*m[1] + b.m[9]*m[5] + b.m[10]*m[9] + b.m[11]*m[13];
	result.m[10] = b.m[8]*m[2] + b.m[9]*m[6] + b.m[10]*m[10] + b.m[11]*m[14];
	result.m[11] = b.m[8]*m[3] + b.m[9]*m[7] + b.m[10]*m[11] + b.m[11]*m[15];
	
	result.m[12] = b.m[12]*m[0] + b.m[13]*m[4] + b.m[14]*m[8] + b.m[15]*m[12];
	result.m[13] = b.m[12]*m[1] + b.m[13]*m[5] + b.m[14]*m[9] + b.m[15]*m[13];
	result.m[14] = b.m[12]*m[2] + b.m[13]*m[6] + b.m[14]*m[10] + b.m[15]*m[14];
	result.m[15] = b.m[12]*m[3] + b.m[13]*m[7] + b.m[14]*m[11] + b.m[15]*m[15];
	return result;
}

// [x x x x]   [x]
// [x x x x] * [x]
// [x x x x]   [x]
// [x x x x]   [x]
vec4 Matrix4x4::operator*(const vec4 mul) const
{
	vec4 result;
	
	result.x = m[0]*mul.x + m[4]*mul.y + m[8]*mul.z + m[12]*mul.w;
	result.y = m[1]*mul.x + m[5]*mul.y + m[9]*mul.z + m[13]*mul.w;
	result.z = m[2]*mul.x + m[6]*mul.y + m[10]*mul.z + m[14]*mul.w;
	result.w = m[3]*mul.x + m[7]*mul.y + m[11]*mul.z + m[15]*mul.w;
	
	return result;
}

vec3 Matrix4x4::operator*(const vec3 mul) const
{
	vec3 result;
	
	result.x = m[0]*mul.x + m[4]*mul.y + m[8]*mul.z + m[12];
	result.y = m[1]*mul.x + m[5]*mul.y + m[9]*mul.z + m[13];
	result.z = m[2]*mul.x + m[6]*mul.y + m[10]*mul.z + m[14];
	
	return result;
}


// Multiply the vector with the transpose of rotation matrix, 
// Then minus the tranlation offset
vec3 Matrix4x4::transformInverse(const vec3& point) const
{
	vec3 result = { m[0] * point.x + m[1] * point.y + m[2] * point.z - m[12],
					m[4] * point.x + m[5] * point.y + m[6] * point.z - m[13],
					m[8] * point.x + m[9] * point.y + m[10]* point.z - m[14]};
	return result;
}



/////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////
//General Algorighm Implementation here.
/////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////
void BubbleSortF(GLfloat* array, GLint size, GLboolean (*MyCompF)(const GLfloat,const GLfloat))
{
	for(GLint i=0;i<size;i++)
		for(GLint j=0;j<size-i-1;j++)
			if(MyCompF(array[j+1],array[j]))
				SWAPF(array[j+1], array[j]);
}

void generalAlgorithm::sortArrayF(GLfloat *array, GLint size, COMPARISON_FLAG flag)
{
	GLboolean (*MyCompF)(const GLfloat, const GLfloat);
	switch (flag) {
		case FLAG_GREATER:
			MyCompF = &GreaterF;
			break;
		case FLAG_LESS:
			MyCompF = &LessF;
			break;
		case FLAG_ABS_GREATER:
			MyCompF = &ABS_GreaterF;
			break;
		case FLAG_ABS_LESS:
			MyCompF = &ABS_LessF;
			break;
		default:
			return;
	}
	BubbleSortF(array, size, MyCompF);
}

